#include "./density.h"


void mds_density_quit( mds_density *rho )
{
	assert(rho);
	MDS_FREE_AS(Real,rho->wF,rho->num_points);
	MDS_FREE_AS(Real,rho->wX,rho->num_points);
	MDS_FREE_AS(Real,rho->f,rho->num_points);
	MDS_FREE_AS(Real,rho->F,rho->num_points);
	MDS_FREE_AS(Real,rho->X,rho->num_points);
	rho->num_particles = 0;
	rho->num_points    = 0;
}

int mds_density_load( mds_density *rho, size_t num_particles)
{
	assert( rho );
	assert(num_particles>0);
	
	rho->num_particles = num_particles;
	rho->num_points    = num_particles + 2;
	
	rho->X  = MDS_CALLOC_AS(Real,rho->num_points);
	rho->F  = MDS_CALLOC_AS(Real,rho->num_points);
	rho->f  = MDS_CALLOC_AS(Real,rho->num_points);
	rho->wX = MDS_CALLOC_AS(Real,rho->num_points);
	rho->wF = MDS_CALLOC_AS(Real,rho->num_points);
	
	if( ! rho->X || ! rho->F || ! rho->f || ! rho->wX || ! rho->wF )
	{
		mds_density_quit( rho );
		return -1;
	}
	else
	{
		/* compute F once and for all */
		const Real dF = MDS_F(1.0)/(1+num_particles);
		size_t i;
		for( i=0; i < rho->num_points; ++i )
		{
			rho->F[i] = i * dF; 
		}
		rho->F[ rho->num_points - 1] = 1;
	}
	
	
	
	return 0;
}

static 
int compare_Real(const void *pa, const void *pb)
{
	const Real *a_ptr = pa;
	const Real *b_ptr = pb;
	
	const Real a = *a_ptr;
	const Real b = *b_ptr;
	
	if(a<b){return -1;}
	else if(a>b){return 1;}
	else {return 0;}
	
}


Real mds_density_F( const mds_density *rho, Real x )
{
	if( x <= rho->X[0] ) 
		return rho->F[0];
	else {
		if( x >= rho->X[ rho->num_points-1] )
			return rho->F[ rho->num_points-1];
		else 
		{
			const Real *lo = rho->X;
			const Real *hi = lo + rho->num_points;
			size_t      j  = 0;
			Real        dx = 0;
			--hi;
			while( (size_t)(hi-lo) > 1 )
			{
				const Real *mid = &lo[ (  (size_t)(hi-lo) ) >> 1 ];
				if( *mid < x )
				{
					lo = mid;
				}
				else
				{
					hi = mid;
				}
			}
			j   = (lo - rho->X); assert( j < (rho->num_points-1) );
			dx  = rho->X[j+1] - rho->X[j];
			if( dx > REAL_EPSILON)
			{
				return rho->F[j] + ((x-rho->X[j])/dx) * ( rho->F[j+1] - rho->F[j] );
			}
			else
				return rho->F[j];
		}
	}
}



void mds_density_build( mds_density *rho, const mds_vector *data, const mds_vector *box, Real width)
{
	assert(rho); 
	assert(data);
	assert(rho->num_points>0);
	assert(rho->X);
	assert(rho->F);
	assert(box);
	assert( width > 0 );
	
	{
		const size_t n = rho->num_particles;
		const size_t m = rho->num_points;
		const Real   L = box->x;
		const Real   xmax = L/2;
		const Real   xmin = -xmax;
		const Real   hw   = width >= L ? L/2 : width/2;
		
		
		/***********************************/
		/** Build the cumulative function */
		/*********************************/
		size_t       i;
		for( i=0; i < n; ++i )
		{
			rho->X[i] = data[i].x;
		}
		rho->X[m-1] = xmin;
		rho->X[m-2] = xmax;
		qsort(rho->X, m, sizeof(Real), compare_Real);
		
		/***********************************/
		/** Build the density function    */
		/*********************************/
		for( i=0; i < m; ++i )
		{
			size_t     j = 0;
			size_t     k = 0;
			
			const Real x_i  = rho->X[i];
			const Real x_lo = x_i - hw;
			const Real x_hi = x_i + hw;
			rho->wX[j] = x_i;
			rho->wF[j] = rho->F[i];
			++j;
			
			/*============*/
			/*= go left =*/
			/*==========*/
			for(k=1;j<n;++k,++j)
			{
				const int wrap = k > i;
				Real      x_k  = 0;
				Real      F_k  = 0;
				if( wrap )
				{
					const size_t l = (i+m-k);
					x_k = rho->X[l]-L;
					F_k = rho->F[l]-1;
				}
				else 
				{
					const size_t l = i-k;
					x_k = rho->X[l];
					F_k = rho->F[l];
				}
				if( x_k > x_lo )
				{
					rho->wX[j] = x_k;
					rho->wF[j] = F_k;
				}
				else 
				{
					break;
				}
				
			}
			
			/*=============*/
			/*= go left =*/
			/*==========*/
			for(k=1;j<n;++k,++j)
			{
				const int wrap = k + i >= m;
				Real      x_k  = 0;
				Real      F_k  = 0;
				if( wrap )
				{
					const size_t l = k+i-m;
					x_k = rho->X[l] + L;
					F_k = 1+rho->F[l];
				}
				else 
				{
					const size_t l = i+k;
					x_k = rho->X[l];
					F_k = rho->F[l];
				}
				
				if( x_k < x_hi )
				{
					rho->wX[j] = x_k;
					rho->wF[j] = F_k;
				}
				else 
				{
					break;
				}
			}
			
			/*============*/
			/*= ghosts  =*/
			/*==========*/
			rho->wX[j] = x_lo;
			if( x_lo < xmin )
			{
				rho->wF[j] = mds_density_F(rho, x_lo+L )-1;
			}
			else
			{
				rho->wF[j] = mds_density_F(rho, x_lo );
			}
			++j;
			
			rho->wX[j] = x_hi;
			if( x_hi > xmax )
			{
				rho->wF[j] = mds_density_F(rho, x_hi-L )+1;
			}
			else
			{
				rho->wF[j] = mds_density_F(rho, x_hi );
			}
			++j;
			
			/*----------------------------------------------------------------*/
			/* compute the least square fit slope                             */
			/*----------------------------------------------------------------*/
			{
				Real sum_x = 0, sum_y = 0, sum_x2 = 0, sum_xy = 0;
				for( k=0; k < j; ++k )
				{
					const Real x_k = rho->wX[k];
					const Real y_k = rho->wF[k];
					sum_x  += x_k;
					sum_y  += y_k;
					sum_x2 += x_k * x_k;
					sum_xy += x_k * y_k; 
				}
				rho->f[i] = (j * sum_xy - (sum_x * sum_y ) ) / ( j * sum_x2 - sum_x * sum_x );
			}
			
		}
		
		/** Rescaling **/
		{
			Real intg = 0;
			for(i=1; i < rho->num_points; ++i )
			{
				intg += 0.5 * (rho->X[i] - rho->X[i-1]) * ( rho->f[i] + rho->f[i-1]);
			}
			fprintf( stderr, "intg=%g\n", intg);
			intg = rho->num_particles/intg;
			for(i=0; i < rho->num_points; ++i )
			{
				rho->f[i] *= intg;
			}
		}
		
	}
	
}


